#include "app.h"
#include "utils.h"
#include "cli.h"

static int prompt_ProcessCommand(void);
static int prompt_Beep(int argc, portCHAR **argv);
static int prompt_Relay(int argc, portCHAR **argv);
static int prompt_ClearTicks(int argc, portCHAR **argv);
static int prompt_Dump (int argc, portCHAR **argv);
static int prompt_ServoConfig(int argc, portCHAR **argv);
static int prompt_Servo(int argc, portCHAR **argv);
static int prompt_ServoEnable(int argc, portCHAR **argv);
static int prompt_ServoOffset(int argc, portCHAR **argv);
static int prompt_Park(int argc, portCHAR **argv);
static int prompt_Hand(int argc, portCHAR **argv);
static int prompt_LogErrorShow (int argc, portCHAR **argv);
static int prompt_LogErrorClear (int argc, portCHAR **argv);
static int prompt_Reconfig (int argc, portCHAR **argv);
static int prompt_STS(int argc, portCHAR **argv);

// ###############################################################################################
// LISTA DOS COMANDOS A SER CHAMADOS PELO CLI

static const tCLI cli_list [] = {
	{ "help",
  			0,  0,
  			CMD_TYPE_FUNCTION, { cli_Help },
  			"Displays this help list", NULL },

	{ "beep",
			1,  1,
  			CMD_TYPE_FUNCTION,  { prompt_Beep },
  			"Play beep sound", "beep <mode: 0(off) at 6>" },

	{ "relay",
  			2,  3,
  			CMD_TYPE_FUNCTION,  { prompt_Relay },
  			"Turn on or off determined relay", "realy <1/2> <on/off> <timeout in secs (OPT)>" },

	{ "dump",
  			2,  2, CMD_TYPE_FUNCTION,  	{ prompt_Dump },
  			"Dump memory",
  			"dump <addr> <size>" },

	{ "clear_ticks",
			0,  0,
  			CMD_TYPE_FUNCTION,  { prompt_ClearTicks },
  			"Clear counter ticks system", NULL },

	{ "servo_config",
  			7,  7,
  			CMD_TYPE_FUNCTION,  { prompt_ServoConfig },
  			"Configurations the servos", "servo_config <servo: 1 at N> <value init> <value min> <value max> <offset> <time progress> <time off>" },

	{ "servo",
  			2,  2,
  			CMD_TYPE_FUNCTION,  { prompt_Servo },
  			"Set new position of the servo", "servo <1..n> <value in us>" },

	{ "servo_offset",
  			2,  2,
  			CMD_TYPE_FUNCTION,  { prompt_ServoOffset },
  			"Set offset particular servo", "servo_offset <servo: 1 at N> <offset in us>" },

	{ "servo_en",
  			2,  2,
  			CMD_TYPE_FUNCTION,  { prompt_ServoEnable },
  			"Enables or not a particular servo", "servo_en <servo: 1 at N> <enabled output pwm: on/off>" },

	{ "park",
  			0,  0,
  			CMD_TYPE_FUNCTION,  { prompt_Park },
  			"Arm prking", "park" },

	{ "hand",
  			1,  1,
  			CMD_TYPE_FUNCTION,  { prompt_Hand },
  			"Open/close hand", "hand <open/close>" },

	{ "log_error_show",
  			0,  0, CMD_TYPE_FUNCTION,  	{ prompt_LogErrorShow },
  			"Show all errors logs",
  			"log_error_show" },

	{ "log_error_clear",
  			0,  0, CMD_TYPE_FUNCTION,  	{ prompt_LogErrorClear },
  			"Clear all errors logs",
  			"log_error_clear" },

	{ "reconfig",
  			0,  0, CMD_TYPE_FUNCTION,  	{ prompt_Reconfig },
  			"Set all parameters to standard",
  			"reconfig" },

	{ "sts",
  			0,  0,
  			CMD_TYPE_FUNCTION,  { prompt_STS },
  			"Shows all status of the ", 	NULL },

	{ NULL, 0,  0, CMD_TYPE_FUNCTION,  	{ NULL },
			NULL,
			NULL },
};

void prompt_Process(void) {
	static int resuslt;
	resuslt = prompt_ProcessCommand();
	switch (resuslt) {
	case errCLI_EXED_ARGS:
		lprintf("System error"CMD_TERMINATOR);
		break;
	case errCLI_CMD_NOT_FOUND:
		lprintf("Unknown command"CMD_TERMINATOR);
    	break;
    case pdFAIL:
    	lprintf("Error in executing the command"CMD_TERMINATOR);
    	break;
    }
}

static int result_prompt;
static u8 buffer_prompt[CLI_CMD_BUFFER_SIZE];
static portCHAR *argv_prompt [CLI_N_ARGS];
static int argc_prompt;

static int prompt_ProcessCommand(void) {
	// CAPTURA COMANDOS DO TERMINAL. SE TECLOU ENTER A FUNÇÃO RETORNA COM A QUANTIDADE DE CHAR DO COMANDO
	result_prompt = cli_GetLine (buffer_prompt, sizeof (buffer_prompt), PROMPT); // Chamo o console para entrada de comandos

	// CHECA FOI ENTRADO COM O COMANDO
	if (result_prompt) {	// checa se há entrada de comandos
   		if (cli_ArgsParse ((char *) buffer_prompt, argv_prompt, sizeof (argv_prompt), &argc_prompt) == errCLI_EXED_ARGS) // Captura os endereços iniciais de cada argumento, contidos no comando, e grava nas posições do argv
   			result_prompt = errCLI_EXED_ARGS;
   		else if (argv_prompt[0])
   			result_prompt = cli_ArgsDispatch(cli_list, argc_prompt, &argv_prompt[0]);							// Executa a função referente ao comando digitado
  	} else result_prompt = pdPASS;

	return result_prompt;
}

static int prompt_Beep(int argc __attribute__ ((unused)), portCHAR **argv) {
	long p1;
	if (!xatoi(&argv[0], &p1)) return pdFAIL;
	beep_Play(p1);
	return pdPASS;
}

static int prompt_Relay(int argc __attribute__ ((unused)), portCHAR **argv ) {
	long p1=0, p2=0, p3=0;

	if (!xatoi(&argv[0], &p1)) return pdFAIL;
	if (strcmp(argv[1], "on") == 0) p2=pdON; else p2=pdOFF;
	if (argc > 2) { if (!xatoi(&argv[2], &p3)) return pdFAIL; }

	int ret = relay_Set(p1-1, p2, p3);

	if (ret == errDEVICE_NOT_FOUND)
		lprintf("ERROR: relay must be 1 or 2"CMD_TERMINATOR);

	return pdPASS;
}

static int prompt_ClearTicks(int argc __attribute__ ((unused)), portCHAR **argv __attribute__ ((unused))) {
	control.ticks = 0;
	return pdPASS;
}

static int prompt_Dump (int argc __attribute__ ((unused)), portCHAR **argv ) {
	long p1, p2;

	if (!xatoi(&argv[0], &p1)) return pdFAIL;
	if (!xatoi(&argv[1], &p2)) return pdFAIL;

	if (p1 < 0) p1 = 0;
	if (p2 < 1) p2 = 256;

	u8 *ptr = (u8*) p1;
	u16 qtd_rd = p2;

	while (qtd_rd) {
  		if (qtd_rd >= 16)
  			// Enquanto o qtd_rd for maior que 15 faz, depois incrementea addr do buffer e da eeprom por 16, e decrementa qtd_rd por 16
  			for( ; qtd_rd>=16; ptr+=16, p1+=16, qtd_rd-=16 )
  				put_dump16(ptr, p1, 16, 16);

		if (qtd_rd)
			put_dump16(ptr, p1, qtd_rd, 16);
  	}

	return pdPASS;
}

static int prompt_Servo(int argc __attribute__ ((unused)), portCHAR **argv ) {
	long p1=0, p2=0;

	if (!xatoi(&argv[0], &p1)) return pdFAIL;
	if (!xatoi(&argv[1], &p2)) return pdFAIL;

	int ret = servo_SetPos(p1,p2);

	tParServo* parServo = (tParServo*)control.servo[p1-1].par;
	if (ret == errDEVICE_NOT_FOUND)
		lprintf("ERROR: Servo between 1 at %d"CMD_TERMINATOR, SERVO_N_MODULE);
	else if ( (ret == errDEVICE_VALUE_OUT_OF_RANGE) || (ret == errDEVICE_VALUE_INVALID) )
		lprintf("ERROR: Position out of range. Value should be %d at %d"CMD_TERMINATOR, parServo->min, parServo->max);

	return pdPASS;
}

static int prompt_ServoEnable(int argc __attribute__ ((unused)), portCHAR **argv ) {
	long p1=0, p2=pdOFF;

	if (!xatoi(&argv[0], &p1)) return pdFAIL;
	if (strcmp(argv[1], "on") == 0) p2=pdON;

	int ret = servus_Enable(p1,p2);

	if (ret == errDEVICE_NOT_FOUND)
		lprintf("ERROR: Servo between 1 at %d"CMD_TERMINATOR, SERVO_N_MODULE);

	return pdPASS;
}

static int prompt_Park(int argc __attribute__ ((unused)), portCHAR **argv __attribute__ ((unused)) ) {
	servo_Parking();
	return pdPASS;
}

static int prompt_Hand(int argc __attribute__ ((unused)), portCHAR **argv ) {
	if (strcmp(argv[0], "close") == 0) servo_Hand(handCLOSE); else servo_Hand(handOPEN);

	return pdPASS;
}


static int prompt_ServoConfig(int argc __attribute__ ((unused)), portCHAR **argv ) {
	long p1, p2, p3, p4, p5, p6, p7;

	if (!xatoi(&argv[0], &p1)) return pdFAIL;
	if (!xatoi(&argv[1], &p2)) return pdFAIL;
	if (!xatoi(&argv[2], &p3)) return pdFAIL;
	if (!xatoi(&argv[3], &p4)) return pdFAIL;
	if (!xatoi(&argv[4], &p5)) return pdFAIL;
	if (!xatoi(&argv[5], &p6)) return pdFAIL;
	if (!xatoi(&argv[6], &p7)) return pdFAIL;

	int ret = servo_Config(p1,p2,p3,p4,p5,p6,p7);
	if (ret == errDEVICE_NOT_FOUND)
		lprintf("ERROR: Servo between 1 at %d"CMD_TERMINATOR, SERVO_N_MODULE);
	else if (ret == errDEVICE_VALUE_INVALID)
		lprintf("ERROR: Position out of range. Value should be %d at %d"CMD_TERMINATOR, minSERVO_US, maxSERVO_US);

	return pdPASS;
}

static int prompt_ServoOffset(int argc __attribute__ ((unused)), portCHAR **argv ) {
	long p1, p2;

	if (!xatoi(&argv[0], &p1)) return pdFAIL;
	if (!xatoi(&argv[1], &p2)) return pdFAIL;

	int ret = servo_Offset(p1,p2);
	if (ret == errDEVICE_NOT_FOUND)
		lprintf("ERROR: Servo between 1 at %d"CMD_TERMINATOR, SERVO_N_MODULE);
	else if (ret == errDEVICE_VALUE_INVALID)
		lprintf("ERROR: Position out of range. Value should be %d at %d"CMD_TERMINATOR, minSERVO_US, maxSERVO_US);

	return pdPASS;
}

static int prompt_LogErrorShow (int argc __attribute__ ((unused)), portCHAR **argv __attribute__ ((unused))) {
	lprintf("ERROR SYSTEM"CMD_TERMINATOR);
	uint x; for(x=0; x<nLOG;x++)
		if (control.err[x].code == 0) lprintf("   slot %d: error none"CMD_TERMINATOR, x);
		else lprintf("   slot %d: error code: %d in count tick: %d"CMD_TERMINATOR, x, control.err[x].code, control.err[x].countTick);

	return pdPASS;
}

static int prompt_LogErrorClear (int argc __attribute__ ((unused)), portCHAR **argv __attribute__ ((unused))) {
	flash_ClearLogError();
	return pdPASS;
}

static int prompt_Reconfig (int argc __attribute__ ((unused)), portCHAR **argv __attribute__ ((unused))) {
	par_ValuesDefaults();
	flash_Save();
	return pdPASS;
}

static int prompt_STS(int argc __attribute__ ((unused)), portCHAR **argv __attribute__ ((unused))) {
	lprintf(CMD_TERMINATOR);
	lprintf(CMD_TERMINATOR);
	lprintf("SYSTEM"CMD_TERMINATOR);
	lprintf("    "PROGAM_NAME);
	lprintf("    ucLibc version "__VERSION CMD_TERMINATOR);
	lprintf("    count TICKS: %u"CMD_TERMINATOR, control.ticks);
	lprintf("    key pressing: %u"CMD_TERMINATOR, control.key);
	lprintf(CMD_TERMINATOR);

	lprintf("SERVOS"CMD_TERMINATOR);
	tParServo* parServo;
	uint x;	for (x=0;x<SERVO_N_MODULE;x++) {
		parServo = (tParServo*)control.servo[x].par;
		lprintf("    servo:%u en:%s pos:%04dus init:%04dus min:%04dus max:%04dus offset:%03d, tp:%02d, toff:%d"CMD_TERMINATOR,
			x+1,
			servus_Enabled(x+1)?"ON ":"OFF",
			control.servo[x].posNew,
			parServo->init,
			parServo->min,
			parServo->max,
			parServo->offset,
			parServo->timeProgress,
			parServo->timeActiveOff
		);
	}
	lprintf(CMD_TERMINATOR);

	lprintf("JOY SERVOS"CMD_TERMINATOR);
	for (x=0;x<SERVO_N_MODULE;x++) {
		lprintf("    servo:%u pos:%04dus"CMD_TERMINATOR, x+1, control.joy[x].pos);
	}
	lprintf(CMD_TERMINATOR);

	for (x=700;x<2600;x+=10) {
		servus_SetPos(6, x);
		delay_ms(10);
	}

	for (x=2600;x>700;x-=10) {
		servus_SetPos(6, x);
		delay_ms(10);
	}

	return pdPASS;
}
